﻿
using GreenXi;
using GreenXi.Help.Can;
using GreenXi.Help.Plc;
using System;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using static GreenXi.GlobalVariables;
using static GreenXi.Help.Can.CanMessage;
using static GreenXi.Help.Curve.ClsJudge;
using static PressSystem_Can.Help.Can.Can;
using static PressSystem_Can.Help.Can.CanHelper;

namespace PressSystem_Can.Help.Can
{
    public class CanHelper
    {
        /// <summary>
        /// 伺服1模式
        /// </summary>
        public static SportMode CurrentSportMode1 { get; set; } = SportMode.PositionMode;
        /// <summary>
        /// 伺服2模式
        /// </summary>
        public static SportMode CurrentSportMode2 { get; set; } = SportMode.PositionMode;

        #region 初始化
        public static void Init()
        {
            Can.Start();
            SetSportMode(1, SportMode.PositionMode);
            SetSportMode(2, SportMode.PositionMode);
            CanHelper.BrakeEnable(1, BrakeEnableType.Open);
            CanHelper.BrakeEnable(2, BrakeEnableType.Open);

            CanHelper.GetStatus(1);
            //使能
            PLC1 plc = new();
            plc.SetPLC("MES_Enable1", true);
            if (CompressorsNumber == 2)
            {
                plc.SetPLC("MES_Enable2", true);
                CanHelper.GetStatus(2);
            }
        }
        #endregion

        #region 运动相关



        /// <summary>
        /// 停止压机
        /// </summary>
        public static bool StopMove(int AxisNum)
        {
            //速度模式停止：运动速度设0
            DisplacementModeStop(AxisNum);
            return true;
        }

        /// <summary>
        /// 开始运动
        /// </summary>
        /// <param name="servoId"></param>
        /// <param name="speed"></param>
        /// <param name="position"></param>

        public static void StartMove(int AxisNum, double speed, double? position = null) //1圈 = 1速度/s
        {
            bool isPositionModeAlready;
            if (position == null) //速度模式
            {
                SetSpeedModeSpeedAndMove(AxisNum, speed);
            }
            else //位置模式
            {
                position *= 1280000 / 2;
                isPositionModeAlready = true;
                if (CurrentSportMode1 != SportMode.PositionMode)
                {
                    isPositionModeAlready = false;
                }
                SetTargetPosition(AxisNum, position);
                SetPositionModeSpeed(AxisNum, speed);
                SendMessage(AxisNum, MessagesCode.ServoOn);
                RisingEdgeTrigger(AxisNum);
            }
        }

        /// <summary>
        /// 重新使能
        /// </summary>
        /// <param name="servoId"></param>
        public static void EnabledServo(int AxisNum)
        {
            SendMessage(AxisNum, MessagesCode.Shutdown);
            SendMessage(AxisNum, MessagesCode.SwitchOn); //Switch on(伺服 Servo On准备)
            SendMessage(AxisNum, MessagesCode.ServoOn); //Enable operation(伺服 使能)
        }


        /// <summary>
        /// 抱闸使能
        /// </summary>
        /// <param name="Type"></param>
        public static void BrakeEnable(int AxisNum, BrakeEnableType Type)
        {
            var msg = "00 00 00 00 00 00 00 00";
            if (Type == BrakeEnableType.Open)
            {
                msg = "03 00 00 00 00 00 00 00";
            }
            SendMessage(1, msg, "0x00000101");
        }

        /// <summary>
        /// 故障复位
        /// </summary>
        /// <param name="servoId"></param>
        public static void ResetServo(int AxisNum)
        {
       
            Can.SendMessage(AxisNum, MessagesCode.ResetHight); //故障复位
            Task.Delay(1);
            Can.SendMessage(AxisNum, MessagesCode.ResetHight); //故障复位
            EnabledServo(AxisNum);

        }
        #endregion


        /// <summary>
        /// Enable operation(伺服 使能)
        /// </summary>
        /// <param name="servoId"></param>
        private static void EnableOperation(int AxisNum)
        {
            Can.SendMessage(AxisNum, MessagesCode.ServoOn); //Enable operation(伺服 使能)
        }

        /// <summary>
        /// 原点复归(上升沿触发)
        /// </summary>
        /// <param name="servoId"></param>
        private static void RisingEdgeTrigger(int AxisNum)
        {
            Can.SendMessage(AxisNum, MessagesCode.RisingEdge);
        }

        /// <summary>
        /// 设置位移模式速度
        /// </summary>
        /// <param name="AxisNum"></param>
        /// <param name="speed"></param>
        private static void SetPositionModeSpeed(int AxisNum, double speed)
        {
            speed *= 1280000 / 2; //1280000 1圈/s  ，对应 1280000puu/s
            Can.SendMessage(AxisNum, Messages.PositionModeSpeed + DecToHexAndInverse(speed));
        }

        /// <summary>
        /// 速度模式设置速度
        /// </summary>

        private static void SetSpeedModeSpeedAndMove(int AxisNum, double speed)
        {
            speed *= 600; //600 1圈/s  ，对应 1280000puu/s
            Can.SendMessage(AxisNum, Messages.SpeedModeSpeed + DecToHexAndInverse(speed));
        }

        /// <summary>
        /// 位移模式停止
        /// </summary>
        private static void DisplacementModeStop(int AxisNum)
        {
            Can.SendMessage(AxisNum, MessagesCode.DisplacementModeStop);
        }


        /// <summary>
        /// 设置目标位置
        /// </summary>
        /// <param name="servoId"></param>
        /// <param name="position"></param>
        private static void SetTargetPosition(int AxisNum, double? position)
        {
            Can.SendMessage(AxisNum, Messages.TargetPosition + DecToHexAndInverse(position ?? 0));
        }


        #region 数据相关
        /// <summary>
        /// 发送位移数据
        /// </summary>
        /// <param name="servoId"></param>
        public static void GetDisplacement(int AxisNum)
        {
            var aa = Messages.Displacement + MessagesCode.Zero;
            Can.SendMessage(AxisNum, Messages.Displacement + MessagesCode.Zero);
        }

        /// <summary>
        /// 实时获取状态
        /// </summary>
        /// <param name="servoId"></param>
        public static void GetStatus(int AxisNum)
        {
         
            Can.SendMessage(AxisNum, MessagesCode.Status );
        }

        /// <summary>
        /// 请求IO输入
        /// </summary>
        /// <param name="servoId"></param>
        public static void PostIOInput()
        {
            Can.SendMessage(1, "00 00 00 00 00 00 00 00", "0x00000301");
        }

        /// <summary>
        /// 设置运动模式
        /// </summary>
        /// <param name="servoId"></param>
        /// <param name="mode"></param>
        public static void SetSportMode(int AxisNum, SportMode mode)
        {

            if (mode == SportMode.PositionMode)
            {
                SendMessage(AxisNum, MessagesCode.DisplacementMode); //设置为位置模式

            }
            else if (mode == SportMode.HomingMode)
            {
                SendMessage(AxisNum, MessagesCode.HomingMode); //设置为复归模式

            }
            CurrentSportMode1 = mode;

        }
        //设置原点复归模式
        /*
         1.设置复归模式
         2.设置原点复归模式
         3.设定寻找原点开关的速度
         4.设定寻找Z PULSE速度
         */
        /// <summary>
        /// 寻零
        /// </summary>
        public static void FindZero(int AxisNum)
        {
            //1.设置复归模式
            SetSportMode(AxisNum, SportMode.HomingMode);
            //2.设置原点复归模式
            SendMessage(AxisNum, MessagesCode.ReturnZeroMode); //设置为速度模式
            //3.设定寻找原点开关的速度                                   
            SendMessage(AxisNum, MessagesCode.ReturnZeroSpeed); //3.设定寻找原点开关的速度
            //4.设定寻找Z PULSE速度
            SendMessage(AxisNum, MessagesCode.ReturnZpulseSpeed);


            SendMessage(AxisNum, MessagesCode.ServoOn);
            RisingEdgeTrigger(AxisNum);
        }
        #endregion




        /// <summary>
        /// 默认1280000   电机 一圈
        /// </summary>
        /// <param name="str"></param>
        /// <returns></returns>
        public static int GetPuu(string str)
        {
            string[] strArr = str.Split(' ');
            Array.Reverse(strArr);
            string reversedString = string.Join(" ", strArr);
            string binaryString = string.Join(
                "",
                reversedString
                    .Split(' ')
                    .Select(hex => Convert.ToString(Convert.ToInt32(hex, 16), 2).PadLeft(8, '0'))
            );
            int decimalValue = Convert.ToInt32(binaryString, 2);

            return decimalValue;
        }

        /// <summary>
        /// 把位移转换成字符串
        /// </summary>
        /// <param name="decimalValue"></param>
        /// <returns></returns>
        public static string GetHexFromStringValue(double decimalValue)
        {
            int Value = Convert.ToInt32(decimalValue * 1280000.00 * 0.5);
            // 将整数转换为十六进制字符串，并移除前缀“0x”
            string hexString = Convert.ToString(Value, 16).ToUpper().PadLeft(8, '0');
            string[] strArr = new string[4];
            for (int i = 0; i < hexString.Length; i += 2)
            {
                string hexPair = hexString.Substring(i, 2);
                strArr[i / 2] = hexPair;
            }
            Array.Reverse(strArr);
            return string.Join(" ", strArr);
        }


        /// <summary>
        /// 对获取的报文进行处理
        /// </summary>
        /// <param name="frameId">帧Id</param>
        /// <param name="msg">报文</param>
        /// <param name="CANInd">第几路Can</param>

        public static void ProcessRecvCANData(string frameId, string msg, int CANInd)
        {
            Message message = GetMessage(msg);

            //报警
            if (frameId == FrameType.报警)
            {
                Int32 index= Convert.ToInt32(message.subindex, 16);
                if (index == 133)
                {
                    index = CANInd == 1 ? 110 : 130;
                }
                if (index == 134)
                {
                    index = CANInd == 1 ? 120 : 140;
                }

                if (CallPolice == 0) CallPolice = index;
            }
            //位移
            if (frameId == FrameType.位移数据)
            {
                ProcessRecvCANServo(frameId, msg, CANInd);
                return;
            }

            //获取压力数据
            if (frameId == FrameType.压力数据)
            {
                //获取位移并赋值
                double Pressure = PuuToPressure(GetPuu(message.data1));
                GlobalVariables.Card_Pressure1 = Pressure;
                double Pressure2 = PuuToPressure(GetPuu(message.data3));
                GlobalVariables.Card_Pressure2 = Pressure2;
                return;
            }
            //IO输入
            if (frameId == FrameType.IO输入)
            {
                var aa = DecToHexAndInverse2(message.functionCode);
                IOHandle(aa);
            }
        }
        /// <summary>
        /// 对获取的报文进行处理
        /// </summary>
        /// <param name="frameId">帧Id</param>
        /// <param name="msg">报文</param>
        public static void ProcessRecvCANServo(string frameId, string msg, int AxisNum)
        {
            Message message = GetMessage(msg);
            //伺服电机上电重新使能
            if (message.functionCode == "00" && message.index == "00 00")
            {
                CanHelper.EnabledServo(AxisNum);
                return;
            }
            //获取状态
           if(message.functionCode=="4B" && message.index == "01 20")
           {
                int puu = GetPuu(message.data);
                //645是限位
                if (CallPolice == 0) CallPolice = puu;

            }
            //获取位移数据
            if (message.functionCode == "43" && message.index == "64 60")
            {
                //获取位移并赋值
                int puu = GetPuu(message.data);
                double Displacement = PuuToDisplacement(puu);

                if (AxisNum == 1)
                {
                    GlobalVariables.Card_Displacement1 = Displacement;
                }
                else
                {
                    GlobalVariables.Card_Displacement2 = Displacement;
                }
                return;
            }

        }

        /// <summary>
        /// 处理I0输入
        /// </summary>
        /// <param name="IONum">输入数字 1--正极限    2--负极限   3--原点   4--急停   5--正极限6--负极限7--原点</param>
        public static void IOHandle(int[] IONum)
        {
            int AxisNum = 1;
            string msg = "";
            //没有变化就直接返回
            if (IONum == IOParameter)
            {
                return;
            }

            for (int i = 0; i < IONum.Length; i++)
            {
                if (IONum[i] == IOParameter[i])
                {
                    continue;
                }
                #region 逻辑处理
                switch (i + 1)
                {
                    //正极限
                    case 1:
                        StopMove(1);
                        if(CallPolice==0)CallPolice = (int)CallPoliceStatus.传感器_到达正限位;
                        break;
                    case 2:
                        StopMove(1);
                        if (CallPolice == 0) CallPolice = (int)CallPoliceStatus.传感器_到达负限位;


                        break;
                    //轴1原点
                    case 3:
                        msg = "2B 0A 22 00 24";
                        break;
                    //急停
                    case 4:
                        //msg = "2B 0B 22 00 21";
                        //StopMove(1);
                        //StopMove(2);
                        break;
                    case 5:
                        StopMove(2);
                        if (CallPolice == 0) CallPolice = (int)CallPoliceStatus.传感器_到达正限位;
                        break;
                    case 6:
                        StopMove(2);
                        if (CallPolice == 0) CallPolice = (int)CallPoliceStatus.传感器_到达负限位;
                        break;
                    case 7:
                        msg = "2B 0A 22 00 24";
                        AxisNum = 2;
                        break;
                }
                string Status = " 00";
                if (IONum[i] == 0)
                {
                    Status = " 01";
                }
                //急停反着的
                //if (i == 3)
                //{
                //    if (IONum[i] == 0)
                //    {
                //        Status = " 00";
                //    }
                //    else
                //    {
                //        Status = " 01";
                //    }
                //}

                if (msg != "")
                {
                    Can.SendMessage(AxisNum, msg + Status + " 00 00");//给1发
                }
                //if (i == 3)
                //{
                //    Can.SendMessage(2, msg + Status + " 00 00");  //急停两个都要发
                //}

                #endregion

            }
            IOParameter = IONum;
        }



        #region 通用类


        public enum BrakeEnableType
        {
            Open,
            Close
        }

        //1：运动模式 2：位置模式
        public enum SportMode
        {
            PositionMode,
            HomingMode
        }

        public static class FrameType
        {
            public static string 压力数据 = "182";
            public static string IO输入 = "301";
            public static string 位移数据 = "581";
            public static string 报警 = "81";
        }

        /// <summary>
        /// 报文
        /// </summary>
        public class Message
        {
            /// <summary>
            /// 功能码
            /// </summary>
            public string functionCode = "00";

            /// <summary>
            /// 索引
            /// </summary>
            public string index = "00 00";

            /// <summary>
            /// 子索引
            /// </summary>
            public string subindex = "00";

            /// <summary>
            /// 数据
            /// </summary>
            public string data = "00 00 00 00";
            /// <summary>
            /// 数据
            /// </summary>
            public string data1 = "00 00";
            /// <summary>
            /// 数据
            /// </summary>
            public string data2 = "00 00";

            public string data3 = "00 00";
        }



        private static double PuuToDisplacement(int puu)
        {
            double a = puu / 1280000.00 * 2.00;
            double res = Math.Round(a, 2);//一圈对应2mm
            return res;
        }
        private static double PuuToPressure(int puu)
        {
            double a = puu * 50 / 65535;
            double res = Math.Round(a, 2);//一圈对应2mm
            return res;
        }
        public static string DecToHexAndInverse(double speed)
        {
            string hexString = Convert.ToString((int)speed, 16).PadLeft(8, '0');

            // 按两位为一组进行倒置，并连接所有组
            string formattedString = string.Join(
                    " ",
                    Enumerable
                        .Range(0, hexString.Length / 2)
                        .Select(i => hexString.Substring(i * 2, 2))
                        .Reverse()
                )
                .ToUpper();

            return formattedString;
        }
        /// <summary>
        /// 转成2进制
        /// </summary>
        /// <param name="speed"></param>
        /// <returns></returns>
        public static int[] DecToHexAndInverse2(string param)
        {
            int hexValue = Convert.ToInt32(param, 16);
            int[] boolArray = new int[8];

            for (int i = 0; i < 8; i++)
            {
                boolArray[i] = (hexValue & (1 << i)) != 0 ? 1 : 0;
            }

            return boolArray;
        }


        public static Message GetMessage(string msg)
        {
            if (msg.Length <= 2)
            {
                return new();
            }
            Message message = new();
            string[] msgArr = msg.Split(' ');
            message.functionCode = msgArr[0];
            message.index = msgArr[1] + " " + msgArr[2];
            message.subindex = msgArr[3];
            message.data = msgArr[4] + " " + msgArr[5] + " " + msgArr[6] + " " + msgArr[7];
            message.data1 = msgArr[0] + " " + msgArr[1];
            message.data2 = msgArr[2] + " " + msgArr[3];
            message.data3 = msgArr[4] + " " + msgArr[5];
            return message;
        }
        #endregion
    }
}
